# 在文件顶部导入必要的模块
import sys

sys.path.append('./py_trees-devel')

import math

global AGENT_PARAMS
AGENT_PARAMS = {
    'BSV_SCAN_RANGE': 30000,
    'USV_SCAN_RANGE': 20000,
    'USV_LOCK_RANGE': 40000,
    'BSV_LOCK_RANGE': 40000,
    'BSV_MIN_VEL': 0,
    'BSV_MAX_VEL': 10,
    'USV_MIN_VEL': 0,
    'USV_MAX_VEL': 10,
    'USV_MAX_ACCEL': 2.0,
    'UAV_MAX_ACCEL': 10.0,
    'UAV_CHARGE_C': 2.5,  # UAV充电倍率，即充满电需要电量消耗的2.5倍时间
    'USV_TURN_RADIUS': 20,
    'UAV_SCAN_RANGE': 60000,
    'UAV_SCAN_ANGLE': math.pi / 6,
    'UAV_MIN_VEL': 20,
    'UAV_MAX_VEL': 150,
    'UAV_TURN_RADIUS': 100
}


# 添加Position类定义
class Position:
    """位置类"""

    def __init__(self, x, y):
        self.x = x
        self.y = y

    def distance_to(self, other):
        """计算到另一个位置的距离"""
        return math.sqrt((self.x - other.x) ** 2 + (self.y - other.y) ** 2)

    def __str__(self):
        return f"Position({self.x:.1f}, {self.y:.1f})"


# HTN-BT框架导入
HTN_FRAMEWORK_AVAILABLE = False
try:
    # import gtpyhop
    import py_trees

    HTN_FRAMEWORK_AVAILABLE = True
    print("BT框架加载成功")
except ImportError as e:
    import traceback

    print(f"BT框架加载失败: {e}")
    print(traceback.format_exc())
    HTN_FRAMEWORK_AVAILABLE = False

import math
import random
import numpy as np
from abc import ABC, abstractmethod
from typing import List, Optional, Dict
from enum import Enum


# 定义智能体状态类型
class AgentState(Enum):
    FREE = "free"  # 空闲或正在执行任务的正常状态
    FROZEN = "frozen"  # 受击后的冻结状态
    STANDBY = "standby"  # 仅限无人机，代表停靠中但电量充足能够执行任务的状态
    CHARGING = "charging"  # 仅限无人机，代表正在充电的状态
    ELIMINATED = "eliminated"  # 无人艇被摧毁/无人机电量耗尽坠毁的状态


# 定义智能体任务类型
class TaskType(Enum):
    IDLE = "idle"
    MOVETOPOSITION = "move_to_position"
    # UAV任务
    EXPLORE = "explore"
    RTB = "rtb"
    CHARGE = 'charge'
    RECKON = "reckon"
    ASSIT_LOCK = "assit_lock"
    # USV任务
    LOCK = "lock"


class Agent(ABC):
    """
    智能体基类
    """
    next_id = 1

    def __init__(self,
                 x: float, y: float, heading: float,
                 faction: str = "white",
                 sensor_range: float = AGENT_PARAMS['USV_SCAN_RANGE'],
                 sensor_angle: float = math.pi,
                 max_speed: float = AGENT_PARAMS['USV_MAX_VEL'],
                 min_speed: float = 0,
                 max_accel: float = AGENT_PARAMS['USV_MAX_ACCEL'],
                 turn_radius: float = AGENT_PARAMS['USV_TURN_RADIUS']):
        """
        智能体基类构造函数 - 支持专门化HTN-BT
        """
        self.id = Agent.next_id
        Agent.next_id += 1

        # Agent动力学参数
        self.x = x
        self.y = y
        self.yaw = heading
        self.vx = 0.0
        self.vy = 0.0
        self.v = 0.0  # 速度标量
        self.omega = 0.0
        self.max_speed = max_speed
        self.min_speed = min_speed
        self.max_accel = max_accel
        self.turn_radius = turn_radius
        self.target_heading = 0.0
        self.target_speed = 0.0
        self.cmd_accel = 0.0
        self.expected_speed = 0.8 * max_speed

        self.faction = faction
        self.sensor_range = sensor_range
        self.sensor_angle = sensor_angle
        self.world = None
        self.freeze = False
        self.state = AgentState.FREE

        # 添加缺失的属性
        self.radius = 50.0  # 智能体半径
        self.health = 100  # 生命值
        self.safe_power_threshold = 30.0
        self.switch_controller_threshold = 100  # 距离目标点减速距离阈值：100m
        self.stop_threshold = 50  # 距离目标点到达距离阈值：50m
        self.dynamic_obstacle_range = 500  # 动态障碍物探测范围：500m
        self.safe_distance = 200.0  # 智能体之间避撞安全距离：200m

        # 初始化BT相关属性
        self.curr_behavior_tree = None  # 当前BT树结构
        # self.htn_initialized = False
        # self.specialized_bt_node = None
        # self.htn_mission_type = None
        # self.htn_state = None
        # self.htn_current_plan = []  # HTN当前计划

        self.max_depth = 1  # 初始化CBBA分配参数: 当前只考虑单任务分配，因此设值为1，后续可能考虑多任务分配情况
        self.task_list = []  # 本地储存的任务位置列表
        self.task_type = TaskType.IDLE  # 智能体任务类型(str)
        self.task_allocated = False  # 智能体是否分配了任务
        self.planned_path = []  # 智能体规划的路径

        # 初始化控制器
        import controller
        self.controller = controller.Controller(self)

    def _create_blackboard(self):
        """创建分布式黑板"""
        try:
            from global_coordinator import DistributedBlackboard

            # 检查是否已经有全局黑板
            if not hasattr(self.__class__, '_shared_blackboard'):
                self.__class__._shared_blackboard = DistributedBlackboard(self.world)
                print(f"创建全局分布式黑板")

            # 为智能体分配黑板引用
            self.blackboard = self.__class__._shared_blackboard
            print(f"智能体 {self.id} 已连接到分布式黑板")

        except Exception as e:

            import traceback
            print(traceback.format_exc())

            print(f"创建黑板失败 {self.id}: {e}")
            # 创建简单的替代黑板
            self.blackboard = None

    def update(self):
        """更新智能体物理状态"""
        if self.world is None:
            return
        else:
            # 更新控制量
            self.controller.follow_trajectory()
            # 转向
            self.yaw += self.omega * self.world.interval
            self.yaw = self._normalized_angle(self.yaw)
            # 速度变化
            self.v += self.cmd_accel * self.world.interval
            # 限制速度区间
            self.v = min(self.max_speed, max(self.min_speed, self.v))
            # 速度向量变化
            self.vx = self.v * self.world.interval * math.cos(self.yaw)
            self.vy = self.v * self.world.interval * math.sin(self.yaw)

            self.x += self.vx * self.world.interval
            self.y += self.vy * self.world.interval

            self.v = math.sqrt(self.vx ** 2 + self.vy ** 2)

    def _normalized_angle(self, angle):
        """规范化角度"""
        return (angle + math.pi) % (2 * math.pi) - math.pi

    @abstractmethod
    def sensor_search(self) -> List['Agent']:
        """在自身传感器范围内搜索目标"""
        pass

    @abstractmethod
    def low_level_decision(self):
        """个体层面的决策行动"""
        pass

    def on_collision(self, other: 'Agent'):
        """碰撞处理"""
        # print(f"{self.__class__.__name__} {self.id} collided with {other.__class__.__name__} {other.id}")

    def on_destroyed(self):
        """智能体被摧毁时的处理"""
        self.health = 0
        self.freeze = True
        self.state = AgentState.ELIMINATED
        if self.world is not None: self.world.remove_agent(self)

    def distance_to(self, other) -> float:
        """计算到另一个智能体或位置的距离"""
        if isinstance(other, Agent):
            return math.sqrt((self.x - other.x) ** 2 + (self.y - other.y) ** 2)
        elif isinstance(other, Position):
            return math.sqrt((self.x - other.x) ** 2 + (self.y - other.y) ** 2)
        elif isinstance(other, tuple) or isinstance(other, list):
            return math.sqrt((self.x - other[0]) ** 2 + (self.y - other[1]) ** 2)

    def angle_to(self, other: 'Agent') -> float:
        """计算到另一个智能体的角度"""
        dx = other.x - self.x
        dy = other.y - self.y
        return math.atan2(dy, dx)

    def set_heading_to(self, other: 'Agent'):
        """设置朝向另一个智能体"""
        self.target_heading = self.angle_to(other)
        self.controller.waypoints = [
            {
                "position": [other.x, other.y, 0.0],
                "arrival_time": self.world.time + 1.0
            }
        ]

    def set_speed(self, velocity: float, direction: float):
        """
        设置速度大小和方向
        :param velocity: 期望速度大小(m/s)
        :param direction: 期望速度方向(rad)
        """
        self.controller.set_speed(velocity, direction)

    def set_waypoints(self, waypoints: List[Dict]):
        """设置航路点"""
        self.controller.waypoints = waypoints

    @abstractmethod
    def execute_bt_decision(self) -> bool:
        """
        执行基于任务分配的BT决策
        
        返回:
        bool: True表示BT执行成功(SUCCESS/RUNNING)，False表示执行失败(FAILURE/创建失败)
        """
        pass

    def _print_bt_node_status(self, node, depth):
        """递归打印BT节点状态"""
        indent = "  " * depth
        node_name = getattr(node, 'name', str(type(node).__name__))
        status = getattr(node, 'status', 'UNKNOWN')
        feedback = getattr(node, 'feedback_message', '')

        print(f"{indent}[{depth}] {node_name}: {status}")
        if feedback:
            print(f"{indent}    反馈: {feedback}")

        # 递归打印子节点
        if hasattr(node, 'children'):
            for child in node.children:
                self._print_bt_node_status(child, depth + 1)


class Drone(Agent):
    """
    无人机类
    """

    def __init__(self,
                 x: float, y: float, heading: float,
                 docking: 'Boat'):
        """
        初始化一个智能体
        :param x: 初始位置x(m)
        :param y: 初始位置y(m)
        :param unused: 是否采用简化运动学模型
        """
        super().__init__(x=x, y=y, heading=heading, faction=docking.faction,
                         sensor_range=AGENT_PARAMS['UAV_SCAN_RANGE'],
                         sensor_angle=math.pi / 6, min_speed=AGENT_PARAMS['UAV_MIN_VEL'],
                         max_speed=AGENT_PARAMS['UAV_MAX_VEL'],
                         max_accel=AGENT_PARAMS['UAV_MAX_ACCEL'])
        self.docking = docking  # 停靠于
        self.need_docking = False
        self.max_power = 7200.0  # 最大续航时间2小时
        self.power = self.max_power  # 剩余续航时间
        self.turn_radius = 100.0  # 最小转弯半径
        self.rtb_target = None  # 补能返航目标
        self.t_avail = 1.0
        self.if_explore_done = False  # 判断是否完成预定探索路线
        self.max_depth = 5

    def update(self):
        if self.world is None: return
        super().update()
        """无人机特定的更新逻辑"""
        self.low_level_decision()
        if not self.freeze:
            # 活跃状态
            if self.power > 0:
                # 消耗续航时间
                self.power -= self.world.interval
                self.state = AgentState.FREE
            else:
                # 电量耗尽强制关机
                self.state = AgentState.ELIMINATED
                self.on_destroyed()
        elif self.docking is not None:
            # 停靠状态
            if self.power < 0.15 * self.max_power:
                self.state = AgentState.CHARGING
            else:
                self.state = AgentState.STANDBY

    def sensor_search(self) -> List['Boat']:
        """
        在无人机传感范围内搜索其可见的目标，仅能够发现敌方舰艇
        :return: 发现的敌方舰艇列表，可为空列表
        """
        result = []
        if self.freeze:
            return result

        # 计算无人机的朝向角度
        if self.vx == 0 and self.vy == 0:
            current_heading = self.target_heading
        else:
            current_heading = math.atan2(self.vy, self.vx)

        for agent in self.world.get_agents_near(self, self.sensor_range):
            if isinstance(agent, Boat) and agent.faction != self.faction:
                angle_to_agent = self.angle_to(agent)
                # 计算与目标角度的差值，并考虑360度循环
                angle_difference = abs(angle_to_agent - current_heading)
                angle_difference = min(angle_difference, 2 * math.pi - angle_difference)
                if angle_difference < self.sensor_angle:
                    result.append(agent)
        return result

    def low_level_decision(self):
        """无人机个体层面的决策行动 - 使用新的BT节点"""

        if self.faction == 'black':
            print("黑方无UAV类型智能体！")
        elif self.faction == 'white':
            if self.rtb_target is not None:
                # 更新t_avail
                if self.rtb_target.drone is not None and self.rtb_target.drone.power < 0.15 * self.rtb_target.drone.max_power:
                    # 以充电到15%为界限
                    t_charge = (0.15 * self.rtb_target.drone.max_power - self.rtb_target.drone.power) * AGENT_PARAMS[
                        'UAV_CHARGE_C']
                else:
                    t_charge = 0
                t_fly = self.distance_to(self.rtb_target) / self.max_speed
                self.t_avail = max(t_charge, t_fly)

            # 2. 白方UAV - 使用新的简化BT系统
            try:
                # 使用新的BT系统
                if self.if_explore_done:
                    if self.execute_bt_decision():
                        # print(f"白方UAV {self.id}: BT决策成功")
                        return

            except Exception as e:
                print(f"白方UAV {self.id}: BT决策异常 {e}, 使用默认行为")
                self._fallback_uav_behavior()

    def _fallback_uav_behavior(self):
        """UAV备用行为"""
        # 默认行为：侦察和电量管理
        # 1. 电量不足时返航
        if self.docking is None and self.power / self.max_power < 0.5:
            self.need_docking = True
            friendly_boats = [agent for agent in self.world.faction_agents.get(self.faction, [])
                              if isinstance(agent, Boat) and agent.drone is None]
            if friendly_boats:
                closest_boat = min(friendly_boats, key=lambda boat: self.distance_to(boat))
                direction = self.angle_to(closest_boat)
                distance = self.distance_to(closest_boat)
                self.task_type = TaskType.RTB
                if distance <= 2000:
                    # 满足降落距离，降低速度
                    self.set_speed(max(self.min_speed, distance * distance / 400000 * self.max_speed), direction)
                else:
                    self.set_speed(self.max_speed, direction)
                return

        # 2. 在空中进行侦察
        if self.docking is None and self.power / self.max_power > 0.3:
            # 侦察敌方区域
            if self.world.step % 120 == 0:  # 每2分钟调整一次航向
                # 朝向敌方区域进行侦察
                enemy_center_x = self.world.width * 0.8  # 假设敌方主要在右侧
                enemy_center_y = self.world.height * 0.5
                dx = enemy_center_x - self.x
                dy = enemy_center_y - self.y
                self.target_heading = math.atan2(dy, dx) + random.uniform(-0.5, 0.5)
                self.yaw = self.target_heading
                self.target_speed = self.max_speed
                self.vx = self.target_speed * math.cos(self.yaw)
                self.vy = self.target_speed * math.sin(self.yaw)

    def take_off(self, x: float = None, y: float = None):
        """
        向指定方向以无人机的速度下限起飞, 未指定方向时起飞方向与舰艇速度方向一致
        :param x: x方向, 会以x, y计算起飞的方向向量
        :param y: y方向, 会以x, y计算起飞的方向向量
        """
        if self.world is not None and isinstance(self.docking, Boat) and self.power > 0:
            self.freeze = False
            if x is None or y is None:
                x, y = self.docking.vx, self.docking.vy
            direction = np.array([x, y])
            norm = np.linalg.norm(direction)
            if norm == 0:
                direction = np.array([0, 1])
            else:
                direction = direction / norm
            self.vx = 20 * direction[0]
            self.vy = 20 * direction[1]
            self.target_heading = math.atan2(y, x)
            self.target_speed = self.max_speed
            self.yaw = self.target_heading
            self.vx = self.target_speed * math.cos(self.yaw)
            self.vy = self.target_speed * math.sin(self.yaw)
            self.docking.drone = None
            self.docking = None

    def on_collision(self, other: Agent):
        """无人机碰撞处理，低速且有空余位置时停靠"""
        if self.need_docking and other is self.rtb_target and isinstance(other, Boat) and other.drone is None:
            speedVec = np.array([self.vx, self.vy])
            speed = np.linalg.norm(speedVec)
            if speed <= self.min_speed + 1:
                # 停靠于目标船
                self.freeze = True
                self.x, self.y, self.vx, self.vy = other.x, other.y, 0, 0
                other.drone = self
                self.docking = other
                self.need_docking = False

    def on_destroyed(self):
        """智能体被摧毁时的处理"""
        self.power = 0
        print(f"{self.faction} drone {self.id} has ran out of energy")
        super().on_destroyed()

    def create_uav_behavior_tree(self) -> py_trees.trees.BehaviourTree:
        """
        为白方UAV创建并返回其行为树
        """
        # --- 创建所有节点实例 ---

        # 根节点
        root = py_trees.composites.Selector(name="UAV_Master_Control", memory=False)

        # 起飞与飞行主干
        takeoff_and_fly = py_trees.composites.Sequence(name="Handle_Takeoff_And_Flight", memory=True)
        is_docked_node = UAV_BTNodes.IsDocked(name="Is_Docked?", agent=self)
        if_has_task_node = UAV_BTNodes.IfHasTask(name="If_Has_Task?", agent=self)
        is_fully_charged_node = UAV_BTNodes.IsFullyCharged(name="Is_Fully_Charged?", agent=self)
        takeoff_action_node = UAV_BTNodes.TakeOff(name="Take_Off", agent=self)

        in_flight_ops = py_trees.composites.Selector(name="In_Flight_Operations", memory=False)

        # 返航分支 (最高优先级)
        rtb_sequence = py_trees.composites.Sequence(name="Emergency_Return_To_Base", memory=True)
        is_low_endurance_node = UAV_BTNodes.IsEnduranceLow(name="Is_Endurance_Low?", agent=self)
        if_attribute_usv = UAV_BTNodes.IfAttributeUSV(name="If_Attribute_USV", agent=self)
        land_on_usv_node = UAV_BTNodes.LandOnUSV(name="Land_On_USV", agent=self)

        # 执行Assit_Lock分支
        assit_lock_mission_profile = py_trees.composites.Sequence(name="Assit_Lock_Mission_Profile", memory=True)
        has_assit_lock_task_node = UAV_BTNodes.HasAssitLockTask(name="Has_Assit_Lock_Task?", agent=self)
        move_to_assit_lock_node = UAV_BTNodes.MoveToAssistLock(name="Move_To_Assit_Lock", agent=self)

        # 更新态势图--Reckon分支
        reckon_mission_profile = py_trees.composites.Sequence(name="Reckon_Mission_Profile", memory=True)
        has_reckon_task_node = UAV_BTNodes.HasReckonTask(name="Has_Reckon_Task?", agent=self)
        update_target_map_node = UAV_BTNodes.MoveToUpdateTask(name="Move_To_Update_Target_Map", agent=self)

        # 默认分支--返回USV充电
        default_mission_profile = py_trees.composites.Sequence(name="Default_Mission_Profile", memory=True)
        if_idle_node = UAV_BTNodes.IfIdle(name="If_Idle?", agent=self)
        idle_mission_profile = py_trees.composites.Selector(name="Idle_Mission_Profile", memory=True)
        check_has_rtb_target = py_trees.composites.Sequence(name="Check_Has_RTB_Target", memory=True)
        if_has_rtb_target = UAV_BTNodes.IfHasRTBTarget(name="If_Has_RTB_Target?", agent=self)
        fly_to_rtb_target = UAV_BTNodes.LandOnUSV(name="Fly_To_RTB_Target", agent=self)
        loiter_here = UAV_BTNodes.LoiterHere(name="Loiter_Here", agent=self)

        # --- 组装行为树 ---

        # 组装起飞分支
        takeoff_and_fly.add_children([is_docked_node, if_has_task_node, takeoff_action_node])

        # 组装返航分支
        rtb_sequence.add_children([is_low_endurance_node, if_attribute_usv, land_on_usv_node])
        assit_lock_mission_profile.add_children([has_assit_lock_task_node, move_to_assit_lock_node])

        # 组装协助侦察任务节点
        reckon_mission_profile.add_children([has_reckon_task_node, update_target_map_node])

        # 组装IDLE任务分支
        check_has_rtb_target.add_children([if_has_rtb_target, fly_to_rtb_target])
        idle_mission_profile.add_children([check_has_rtb_target, loiter_here])
        default_mission_profile.add_children([if_idle_node, idle_mission_profile])

        # 组装飞行中操作
        in_flight_ops.add_child(rtb_sequence)
        in_flight_ops.add_child(assit_lock_mission_profile)
        in_flight_ops.add_child(reckon_mission_profile)
        in_flight_ops.add_child(default_mission_profile)

        # 组装根节点
        root.add_children([takeoff_and_fly, in_flight_ops])

        # 创建并保存行为树
        self.curr_behavior_tree = py_trees.trees.BehaviourTree(root=root)

    def execute_bt_decision(self) -> bool:
        try:
            # 1. 初始化BT（如果还没有创建）
            if self.curr_behavior_tree is None:
                self.create_uav_behavior_tree()

            # 2. 执行BT
            if self.curr_behavior_tree is not None:

                self.curr_behavior_tree.tick()

                # self._print_bt_node_status(self.curr_behavior_tree.root, 0)

                result = self.curr_behavior_tree.root.status
                # result = self.curr_behavior_tree.update()
                # print("执行BT")
                # 3. 根据BT执行结果决定返回值
                if result == py_trees.common.Status.RUNNING:
                    # BT正在执行中，表示成功
                    # print("RUNNING")
                    return True
                elif result == py_trees.common.Status.SUCCESS:
                    # BT执行完成，表示成功
                    # print("SUCCESS")
                    return True
                elif result == py_trees.common.Status.FAILURE:
                    # BT执行失败，可能没有任务或路径规划失败
                    # print("FAILURE")
                    # self._print_bt_node_status(self.curr_behavior_tree.root, 0)
                    return False
                elif result == py_trees.common.Status.INVALID:
                    # BT节点未初始化或未激活
                    print("INVALID - BT节点未初始化，尝试重新创建")
                    # 重新创建BT
                    self.curr_behavior_tree = None
                    self.create_uav_behavior_tree()
                    return False
            else:
                import traceback
                print(traceback.format_exc())
                # BT创建失败
                print(f"智能体 {self.id}: BT创建失败")
                return False

        except Exception as e:
            # import traceback
            # print(traceback.format_exc())
            # print(f"智能体 {self.id}: BT执行异常: {e}")
            # return False
            import sys, traceback
            print(f"[BT异常][Agent {self.id}] tick() 内抛出异常：{e}", file=sys.stderr, flush=True)
            traceback.print_exc(file=sys.stderr)
            sys.stderr.flush()
            # 调试时可打开下一行，让仿真直接中断并在终端显示完整回溯
            # raise
            return False


class Boat(Agent):
    """
    舰船类
    """

    def __init__(self,
                 x: float, y: float, heading: float,
                 faction: str,
                 sensor_range: float,
                 lock_range: float,
                 has_drone: bool = False):
        """
        初始化一个无人艇
        :param x: 初始位置x(m)
        :param y: 初始位置y(m)
        :param faction: 阵营
        :param sensor_range: 探测半径(m)
        :param lock_range: 锁定半径(m)
        """
        super().__init__(x=x, y=y, heading=heading, faction=faction, sensor_range=sensor_range,
                         max_speed=AGENT_PARAMS['USV_MAX_VEL'] if faction == 'white' else AGENT_PARAMS['BSV_MAX_VEL'])
        self.lock_count_down = 300.0  # 每次锁定用时五分钟
        self.hit_probability = 1.0  # 单次锁定成功命中的概率
        self.freeze_recover_time = 300
        self.lock_range = lock_range  # 有效锁定范围
        self.drone: Optional[Drone] = Drone(x, y, heading, self) if has_drone else None
        if self.drone is not None:
            self.drone.freeze = True
            self.drone.rtb_target = self
        self._target: Optional[Boat] = None  # 正在锁定的目标
        self.lock_timer = 0.0  # 针对单个目标的已锁定时间
        self.freeze_recover_timer = 0
        self.collision_count = 0  # 记录无人艇的碰撞次数
        self.turn_radius = AGENT_PARAMS['USV_TURN_RADIUS']  # 最小转弯半径
        self.path_plan_cnt = 0  # 路径重规划倒计时
        self.path_plan_target = None  # 路径重规划目标
        self.path_plan_interval = 120  # 路径重规划间隔
        self.threshold_distance = None  # 呼叫UAV协同探测的距离阈值

        self.set_waypoints([{
            "position": [100000, self.y, 0.0],  # 初始位置
            "arrival_time": 0.0  # 初始到达时间
        },
            {
                "position": [0, self.y + 1000, 0.0],  # 终点位置
                "arrival_time": 0.0
            }
        ])
        self.set_speed(10, 0)

    def update(self):
        if self.world is None: return
        super().update()
        """舰船特定的更新逻辑"""
        # 带着靠泊的无人机移动
        if self.drone is not None:
            if self.drone.world is None: self.world.add_agent(self.drone)
            self.drone.x, self.drone.y = self.x, self.y
            self.drone.vx, self.drone.vy = 0, 0
            if not self.freeze:
                # 为停靠的无人机充电(用时2.5倍无人机续航时间)
                self.drone.power += self.world.interval / AGENT_PARAMS['UAV_CHARGE_C']
                self.drone.power = min(self.drone.max_power, self.drone.power)  # 防止过充
        # 若被瘫痪则不进行后续操作
        if self.freeze:
            self.state = AgentState.FROZEN
            self.freeze_recover_timer += self.world.interval
            # 从瘫痪中恢复
            if self.freeze_recover_timer >= self.freeze_recover_time:
                self.freeze = False
                self.freeze_recover_timer = 0
            else:
                return
        self.state = AgentState.FREE
        # 个体决策
        self.low_level_decision()
        # 自动尝试锁定攻击活跃目标
        if self._target is not None:
            self.lock_timer += self.world.interval  # 更新锁定计时器
            self.lock(self._target)  # 保持对目标的锁定并尝试攻击

    def sensor_search(self) -> List['Boat']:
        """
        在舰船传感范围内搜索其可见的目标，仅能够发现敌方舰艇
        :return: 发现的敌方舰艇列表，可为空列表
        """
        result = []
        if self.freeze: return result  # 瘫痪期间无动作
        potentials = self.world.get_agents_near(self, self.sensor_range)
        for agent in potentials:
            if isinstance(agent, Boat) and agent.faction != self.faction:
                result.append(agent)
        return result

    def low_level_decision(self):
        """船只个体层面的决策行动 - 使用新的BT节点"""
        if self.freeze:
            return

        # 1. 黑方USV - 默认行为
        if self.faction == 'black':
            # 如果没有设置初始速度，立即设置
            if self.target_speed == 0:
                # 朝向世界中心进行防御
                center_x = self.world.width / 2
                center_y = self.world.height / 2
                dx = 200 - self.x
                dy = center_y - self.y
                self.target_heading = math.atan2(dy, dx) + random.uniform(-0.3, 0.3)
                self.yaw = self.target_heading
                self.target_speed = self.max_speed
                self.vx = self.target_speed * math.cos(self.target_heading)
                self.vy = self.target_speed * math.sin(self.target_heading)

                print(
                    f"黑方USV {self.id}: 初始化移动，朝向({dx:.0f}, {dy:.0f}), 航向角{self.target_heading:.2f}, 速度{self.target_speed:.1f}m/s")

            # 搜索威胁并尝试锁定
            threats = self.sensor_search()
            if threats:
                closest_threat = min(threats, key=lambda t: self.distance_to(t))
                if self.distance_to(closest_threat) <= self.lock_range:
                    self.lock(closest_threat)

        # 2. 白方船只 - 使用新的简化BT系统
        elif self.faction == 'white':
            try:
                # 使用新的BT系统
                if self.execute_bt_decision():
                    # print(f"白方USV {self.id}: BT决策成功")
                    if self.world.step % 30 == 0 and self.drone is not None and self.drone.power > 300:
                        # 检查前方是否存在危险
                        safe = self.world.faction_maps[self.faction].can_pass([self.x, self.y])
                        if safe:
                            safe = self.world.faction_maps[self.faction].can_pass(
                                [self.x + self.vx * 60, self.y + self.vy * 60])
                        if not safe:
                            self.launch_drone(self.vx, self.vy)  # 立即发射无人机侦察危险区
                    # BT执行成功后，仍然检查自身周围威胁并尝试锁定
                    threats = self.sensor_search()
                    if threats:
                        closest_threat = min(threats, key=lambda t: self.distance_to(t))
                        if self.distance_to(closest_threat) <= self.lock_range:
                            self.lock(closest_threat)
                    return
                else:
                    pass
                    # BT决策失败时使用默认行为
                    # print(f"白方USV {self.id}: BT决策失败，使用默认行为")
                    # self._fallback_usv_behavior()

            except Exception as e:
                pass
                # print(f"白方USV {self.id}: BT决策异常 {e}, 使用默认行为")
                # self._fallback_usv_behavior()

    def _fallback_usv_behavior(self):
        """USV备用行为"""
        # 如果没有设置初始速度，立即设置
        if self.target_speed == 0:
            # 朝向世界左侧进行突防
            target_x = self.world.width * 0.3  # 突防目标
            target_y = self.world.height * 0.5
            dx = target_x - self.x
            dy = target_y - self.y
            self.target_heading = math.atan2(dy, dx) + random.uniform(-0.2, 0.2)
            self.yaw = self.target_heading
            self.target_speed = self.max_speed
            self.vx = self.target_speed * math.cos(self.target_heading)
            self.vy = self.target_speed * math.sin(self.target_heading)

            print(
                f"{self.faction} USV {self.id}: 初始化突防，朝向({target_x:.0f}, {target_y:.0f}), 速度{self.target_speed:.1f}m/s")

        # 搜索威胁并考虑攻击
        threats = self.sensor_search()
        if threats:
            usv_threats = [t for t in threats if isinstance(t, Boat)]
            if usv_threats:
                closest_usv = min(usv_threats, key=lambda t: self.distance_to(t))
                if self.distance_to(closest_usv) <= self.lock_range:
                    self.lock(closest_usv)

    def lock(self, target: 'Boat'):
        """
        锁定指定目标，调用此方法以尝试对目标进行攻击或切换目标
        """
        if self.freeze: return  # 瘫痪期间无动作
        # 检查当前目标与正在锁定的目标是否一致，仅能够锁定范围内的舰艇目标
        if isinstance(target, Boat) and target is not self._target and self.distance_to(target) <= self.lock_range:
            # 更新目标并重置锁定计时器
            self._target = target
            self.lock_timer = 0
            self.world.faction_attacking[self.faction].add(self._target.id)
        elif self._target.health > 0 and self.distance_to(self._target) <= self.lock_range:
            self._target.on_locked([self.x - self._target.x, self.y - self._target.y])
            self.world.faction_attacking[self.faction].add(self._target.id)
            if self.lock_timer >= self.lock_count_down:
                # 锁定足够久后尝试触发命中目标
                if random.random() < self.hit_probability:
                    self.on_hit(target)
                self.lock_timer = 0  # 无论是否命中都重置锁定计时器
        else:
            # 清除无效目标
            self._target = None
            self.lock_timer = 0

    def boat_lock_s1(self):
        from intercept import calculate_interception_point
        # USV锁定目标————阶段1
        # 目标不明确，在周围等待
        target_id = self.task_list[0]['task_id']
        # if self.task_list:
        #     target_id = self.task_list[0]['task_id']
        # elif self._target:
        #     target_id = self._target.id
        # else:
        #     return
        if target_id != self.path_plan_target:
            self.path_plan_target = target_id
            self.path_plan_cnt = 0

        target = self.world.faction_maps[self.faction].target_maps[target_id]
        position = [self.x, self.y]
        self.path_plan_cnt -= 1
        if self.path_plan_cnt <= 0:
            self.path_plan_cnt = self.path_plan_interval
            # 微分博弈计算拦截位置
            # _, _, intercept_point, _, _ = regional_interception_game(target.position, target.velocity, [self.x, self.y])
            intercept_point, can_intercept = calculate_interception_point(target.position, target.velocity,
                                                                          lock_range=AGENT_PARAMS['USV_LOCK_RANGE'],
                                                                          friend_pos=[self.x, self.y])
            if intercept_point is None or np.isnan(intercept_point).any():
                # 微分博弈失效时的备用计算方法
                print(f"USV {self.id}: 无法使用微分博弈，使用备用方法")
                tar_ori = target.last_detect_position
                tar_radius = (target.time - target.last_detected) * target.max_speed * self.world.interval
                if np.linalg.norm(np.array(tar_ori) - np.array(position)) < 1e-6:
                    direction = np.array([-1, 0])
                else:
                    direction = (np.array(tar_ori) - np.array(position)) / np.linalg.norm(
                        (np.array(tar_ori) - np.array(position)))
                intercept_point = np.array(tar_ori) - direction * (tar_radius + self.lock_range)
            if intercept_point[0] < self.world.defense_line_x and not can_intercept:
                # 拦截点不超过防御线
                t = max(0.0, self.x - 550000.0) / 250000
                intercept_point[0] = (1 - t) * self.world.defense_line_x + t * intercept_point[0]
            # A*寻路
            # multi_map = self.world.faction_maps[self.faction]
            # path, _ = path_plan.path_plan(width=multi_map.grid_width, height=multi_map.grid_height,
            #                               map=multi_map.obstacle_map,
            #                               start=multi_map.real_to_grid(position),
            #                               goal=multi_map.real_to_grid(intercept_point),
            #                               grid_resolution=multi_map.grid_resolution,
            #                               time_start=self.world.time, time_goal=self.world.time + 1)
            # self.set_waypoints(path)  # 设置航行路径
            self.set_waypoints([{"position": [intercept_point[0], intercept_point[1], 0.0], "arrival_time": 0.0}])  # 直航目标点

    def boat_lock_s2(self):
        from intercept import calculate_interception_point
        # USV锁定目标————阶段2
        # 目标明确，在35km锁定

        target_id = self.task_list[0]['task_id']
        # if self.task_list:
        #     target_id = self.task_list[0]['task_id']
        # elif self._target:
        #     target_id = self._target.id
        # else:
        #     return
        if target_id != self.path_plan_target:
            self.path_plan_target = target_id
            self.path_plan_cnt = 0

        target = self.world.faction_maps[self.faction].target_maps[target_id]
        position = [self.x, self.y]
        target_recon_radius = AGENT_PARAMS["BSV_SCAN_RANGE"]

        lock_dist = (target_recon_radius + self.lock_range) / 2
        lock_area_width = abs(target_recon_radius - self.lock_range)
        lock_area_half = lock_area_width / 2

        if np.linalg.norm(np.array(target.position) - np.array(position)) > self.lock_range:
            # ag2tar = np.array(target.position) - np.array(position)
            # self.set_speed(self.max_speed, np.arctan2(ag2tar[1], ag2tar[0]))
            self.path_plan_cnt -= 1
            if self.path_plan_cnt <= 0:
                # 微分博弈计算拦截位置
                # _, _, intercept_point, _, _ = regional_interception_game(target.position, target.velocity, [self.x, self.y])
                intercept_point, can_intercept = calculate_interception_point(target.position, target.velocity,
                                                                              [self.x, self.y])
                if intercept_point is None or np.isnan(intercept_point).any():
                    # 微分博弈失效时的备用计算方法
                    print(f"USV {self.id}: 无法使用微分博弈，使用备用方法")
                    tar_radius = (target.time - target.last_detected) * target.max_speed * self.world.interval
                    if np.linalg.norm(np.array(target.position) - np.array(position)) < 1e-6:
                        direction = np.array([-1, 0])
                    else:
                        direction = (np.array(target.position) - np.array(position)) / np.linalg.norm((np.array(target.position) - np.array(position)))
                    intercept_point = np.array(target.position) - direction * (tar_radius + self.lock_range)
                if intercept_point[0] < self.world.defense_line_x and not can_intercept:
                    # 拦截点不超过防御线
                    t = max(0.0, self.x - 550000.0) / 250000
                    intercept_point[0] = (1 - t) * self.world.defense_line_x + t * intercept_point[0]
                # multi_map = self.world.faction_maps[self.faction]
                # path, _ = path_plan.path_plan(width=multi_map.grid_width, height=multi_map.grid_height,
                #                               map=multi_map.obstacle_map,
                #                               start=multi_map.real_to_grid(position),
                #                               goal=multi_map.real_to_grid(intercept_point),
                #                               grid_resolution=multi_map.grid_resolution,
                #                               time_start=self.world.time, time_goal=self.world.time + 1)
                # self.set_waypoints(path)  # 设置航行路径
                self.set_waypoints([{"position": [intercept_point[0], intercept_point[1], 0.0], "arrival_time": 0.0}])  # 直航目标点
        else:
            # 使用人工势场法计算速度方向
            position = np.array([self.x, self.y])
            target_pos = np.array(target.position)

            # 计算吸引势场（朝向主要目标）
            if np.linalg.norm(target_pos - position) < 1e-6:
                attractive_force = np.array([-1, 0])
            else:
                attractive_force = (target_pos - position) / np.linalg.norm(target_pos - position)

            # 计算排斥势场（远离其他敌方目标）
            repulsive_force = np.array([0.0, 0.0])
            repulsion_radius = AGENT_PARAMS['BSV_LOCK_RANGE']

            # 从态势图中获取40km内所有存活目标
            for target_map in self.world.faction_maps[self.faction].target_maps.values():
                if target_map.belief > 0:  # 存活目标
                    other_target_pos = np.array(target_map.position)
                    distance_to_other = np.linalg.norm(other_target_pos - position)

                    # 如果在范围内，计算排斥力
                    if repulsion_radius > distance_to_other > 1e-6:
                        # 排斥力方向：从其他目标指向本船
                        repulsion_direction = (position - other_target_pos) / distance_to_other
                        # 排斥力大小：距离越近，排斥力越大，37.5km时达到1
                        repulsion_magnitude = (repulsion_radius - distance_to_other) / 2500
                        repulsive_force += repulsion_direction * repulsion_magnitude

            # 合成总力场
            total_force = attractive_force + repulsive_force

            # 归一化总力场
            force_magnitude = np.linalg.norm(total_force)
            if force_magnitude > 1e-6:
                total_force = total_force / force_magnitude

            # 计算期望速度方向
            angle_exp = np.arctan2(total_force[1], total_force[0])

            # A*计算速度大小（保持原有的速度计算逻辑）
            # if np.linalg.norm(target_pos - position) < 1e-6:
            #     direction = np.array([-1, 0])
            # else:
            #     direction = (target_pos - position) / np.linalg.norm(target_pos - position)
            #
            # speed_exp_rel = direction * self.max_speed / lock_area_half * (
            #         np.linalg.norm(target_pos - position) - lock_dist)
            # speed_exp = np.array(target.velocity) + speed_exp_rel
            # speed = math.sqrt(speed_exp[0] ** 2 + speed_exp[1] ** 2)

            self.set_speed(min(1, force_magnitude) * self.max_speed, angle_exp)

    def launch_drone(self, x: float = None, y: float = None):
        """
        向指定方向以无人机的速度下限发射搭载的无人机, 未指定方向时发射方向与舰艇速度方向一致
        :param x: x方向, 会以x, y计算发射的方向向量
        :param y: y方向, 会以x, y计算发射的方向向量
        """
        if self.drone is not None and self.drone.power > 0:
            self.drone.freeze = False
            if x is None or y is None:
                x, y = self.vx, self.vy
            direction = np.array([x, y])
            norm = np.linalg.norm(direction)
            if norm == 0:
                direction = np.array([0, 1])
            else:
                direction = direction / norm
            self.drone.vx = 20 * direction[0]
            self.drone.vy = 20 * direction[1]
            self.drone.target_heading = math.atan2(y, x)
            self.target_speed = self.drone.max_speed
            self.drone.docking = None
            self.drone = None

    def on_hit(self, target: Agent):
        """命中目标时的处理"""
        target.freeze = True
        if target.health > 0:
            target.health -= 50  # 造成伤害
            # 记录命中
            if self.faction == 'white':
                self.world.black_freeze_count += 1
            else:
                self.world.white_freeze_count += 1
        if target.health <= 0:
            self.on_kill(target)
            target.on_destroyed()
        print(f"{self.faction} boat {self.id} has hit {target.faction} boat {target.id}")

    def on_kill(self, target: Agent):
        """摧毁目标时的处理"""
        print(f"{self.faction} boat {self.id} has destroyed {target.faction} boat {target.id}")
        target.state = AgentState.ELIMINATED
        if self.faction == 'white':
            self.world.global_coordinator.last_usv_lock_allocation_time = -114514  # 击杀目标时立即触发一次重分配
            self.world.global_coordinator.last_uav_charge_allocation_time = -114514
        self.world.faction_maps[self.faction].remove_target(target.id)  # 移除已被摧毁的目标
        if self._target is target: self._target = None
        pass

    def on_locked(self, source: List[float]):
        """
        舰船被锁定时的处理

        :param source: 相对世界坐标的向量，指向锁定信号的来源
        :type source: List[float]，如：[1, 5]
        """
        if self.faction == 'white':
            x, y = source[0], source[1]  # 相对世界坐标的向量，指向信号来源
            # self.blackboard.update_enemy_situation_maps(self, x, y)
            self.launch_drone(x, y)  # 弹射无人机避免损失，同时侦察目标
            self.set_speed(self.max_speed, math.atan2(-y, -x))  # 远离锁定来源方向移动
        pass

    def on_collision(self, other: Agent):
        """舰船碰撞处理"""
        super().on_collision(other)
        if self.faction == 'white' and isinstance(other, Boat):
            self.world.white_collision_count += 1

    def on_destroyed(self):
        """智能体被摧毁时的处理"""
        # 一并摧毁停靠的无人机
        if self.drone is not None: self.drone.on_destroyed()
        super().on_destroyed()

    def create_usv_behavior_tree(self) -> py_trees.trees.BehaviourTree:
        """
        为USV创建并返回其行为树
        实现移动至目标位置和机会性攻击的并行行为
        """
        # --- 创建所有节点实例 ---

        # 根节点
        root = py_trees.composites.Parallel(
            name=f"USV_{self.id}_Master_Control",
            policy=py_trees.common.ParallelPolicy.SuccessOnOne()
        )

        # 路径规划与移动分支
        lock_path_planning_and_movement = py_trees.composites.Sequence(name="LockTask_Path_Planning_And_Movement",
                                                                       memory=True)

        # 锁定任务移动分支
        check_target_detected = py_trees.composites.Selector(name="Is_Target_Detected", memory=False)
        when_target_detected = py_trees.composites.Sequence(name="Is_Target_Detected", memory=False)
        if_has_tasklist = USV_BTNodes.IfTaskTypeMatches(name="If_Has_TaskList", agent=self, task_type=TaskType.LOCK)
        if_target_detected = USV_BTNodes.IfTargetDetected(name="If_Target_Detected", agent=self)
        loiter_around_target = USV_BTNodes.USVLoiterAroundTarget(name="Loiter_Around_Target", agent=self)
        lock_at_35km = USV_BTNodes.USVLockAt35km(name="Lock_At_35km", agent=self)

        # 机会性攻击分支（Default 疯狗模式）
        opportunistic_combat = py_trees.composites.Sequence(name="Opportunistic_Combat_Operations", memory=True)
        acquire_maddog_target = USV_BTNodes.AcquireNearestBSV(name="AcquireNearestBSV", agent=self)

        # --- 组装行为树 ---

        # 组装锁定任务移动分支
        when_target_detected.add_children([if_target_detected, lock_at_35km])
        check_target_detected.add_children([when_target_detected, loiter_around_target])
        lock_path_planning_and_movement.add_children([if_has_tasklist, check_target_detected])

        # 组装机会性攻击分支
        opportunistic_combat.add_children([acquire_maddog_target])

        # 组装根节点 (并行执行移动和攻击)
        root.add_children([opportunistic_combat, lock_path_planning_and_movement])

        # 创建并保存行为树
        self.curr_behavior_tree = py_trees.trees.BehaviourTree(root=root)
        return self.curr_behavior_tree

    def execute_bt_decision(self) -> bool:
        try:
            # 1. 初始化BT（如果还没有创建）
            if self.curr_behavior_tree is None:
                self.create_usv_behavior_tree()

            # 2. 执行BT
            if self.curr_behavior_tree is not None:

                self.curr_behavior_tree.tick()

                # self._print_bt_node_status(self.curr_behavior_tree.root, 0)

                result = self.curr_behavior_tree.root.status
                # result = self.curr_behavior_tree.update()
                # print("执行BT")
                # 3. 根据BT执行结果决定返回值
                if result == py_trees.common.Status.RUNNING:
                    # BT正在执行中，表示成功
                    # print("RUNNING")
                    return True
                elif result == py_trees.common.Status.SUCCESS:
                    # BT执行完成，表示成功
                    # print("SUCCESS")
                    return True
                elif result == py_trees.common.Status.FAILURE:
                    # BT执行失败，可能没有任务或路径规划失败
                    # print("FAILURE")
                    # self._print_bt_node_status(self.curr_behavior_tree.root, 0)
                    return False
                elif result == py_trees.common.Status.INVALID:
                    # BT节点未初始化或未激活
                    print("INVALID - BT节点未初始化，尝试重新创建")
                    # 重新创建BT
                    self.curr_behavior_tree = None
                    self.create_usv_behavior_tree()
                    return False
            else:
                import traceback
                print(traceback.format_exc())
                # BT创建失败
                print(f"智能体 {self.id}: BT创建失败")
                return False

        except Exception as e:
            # import traceback
            # print(traceback.format_exc())
            # print(f"智能体 {self.id}: BT执行异常: {e}")
            # return False
            import sys, traceback
            print(f"[BT异常][Agent {self.id}] tick() 内抛出异常：{e}", file=sys.stderr, flush=True)
            traceback.print_exc(file=sys.stderr)
            sys.stderr.flush()
            # 调试时可打开下一行，让仿真直接中断并在终端显示完整回溯
            # raise
            return False

##############################################################################
# 【专门化BT节点集合】
##############################################################################

class USV_BTNodes:
    """专门化行为树 (Behavior Tree) 节点"""

    # --- 条件节点 (Conditions) ---

    class AcquireNearestBSV(py_trees.behaviour.Behaviour):
        """选择最近的黑方BSV，写入agent.task_list[0]['task_id']。
        若未探测到目标且无历史target_id，则FAILURE。
        """

        def __init__(self, name: str, agent: Boat):
            super().__init__(name)
            self.agent = agent

        def update(self) -> py_trees.common.Status:
            if self.agent._target is not None:
                self.agent.task_type = TaskType.LOCK
                task_info = {
                    'task_id': self.agent._target.id,
                    'task_type': 'LOCKING',
                    'target_position': [self.agent._target.x, self.agent._target.y],
                    'target_velocity': [self.agent._target.vx, self.agent._target.vy],
                    'threat_score': 100,
                    'status': 'assigned'
                }
                self.agent.task_list = [task_info]
                return py_trees.common.Status.SUCCESS
            candidates = []
            for candidate in self.agent.world.faction_detected[self.agent.faction]:
                if isinstance(candidate, Boat) and self.agent.distance_to(candidate) < self.agent.lock_range:
                    candidates.append(candidate)
            if candidates:
                target = min(candidates, key=lambda t: self.agent.distance_to(t))
                self.feedback_message = f"获取最近BSV目标: {target.id}"
                task_info = {
                    'task_id': target.id,
                    'task_type': 'LOCKING',
                    'target_position': [target.x, target.y],
                    'target_velocity': [target.vx, target.vy],
                    'threat_score': 100,
                    'status': 'assigned'
                }
                self.agent.task_list = [task_info]
                self.agent.task_type = TaskType.LOCK
                self.agent.lock(target)
                return py_trees.common.Status.SUCCESS
            else:
                candidates = []
                for target in self.agent.world.faction_maps[self.agent.faction].target_maps.values():
                    if target.belief > 0 and self.agent.distance_to(target.position) < self.agent.lock_range + 10000:
                        candidates.append(target)
                # 如果在target_maps中找到候选目标，则选择最近的一个
                if candidates:
                    target = min(candidates, key=lambda t: self.agent.distance_to(t.position))
                    self.feedback_message = f"获取最近BSV目标: {target.target_id}"
                    task_info = {
                        'task_id': target.target_id,
                        'task_type': 'LOCKING',
                        'target_position': target.position,
                        'target_velocity': target.velocity,
                        'threat_score': 100,
                        'status': 'assigned'
                    }
                    self.agent.task_list = [task_info]
                    self.agent.task_type = TaskType.LOCK
                    return py_trees.common.Status.SUCCESS

            self.feedback_message = "等待被分配拦截目标"
            return py_trees.common.Status.RUNNING

    class IfTaskTypeMatches(py_trees.behaviour.Behaviour):
        def __init__(self, name: str, agent: Agent, task_type: TaskType):
            super().__init__(name)
            self.agent = agent
            self.task_type = task_type

        def update(self) -> py_trees.common.Status:
            if self.agent.task_list and self.agent.task_type == self.task_type:
                return py_trees.common.Status.SUCCESS
            else:
                return py_trees.common.Status.FAILURE

    class IfTargetDetected(py_trees.behaviour.Behaviour):
        def __init__(self, name: str, agent: Boat):
            super().__init__(name)
            self.agent = agent

        def update(self) -> py_trees.common.Status:
            target_id = self.agent.task_list[0]['task_id']
            target = self.agent.world.faction_maps[self.agent.faction].target_maps[target_id]
            if target.time == target.last_detected:
                return py_trees.common.Status.SUCCESS
            else:
                return py_trees.common.Status.FAILURE

    # --- 动作节点 (Actions) ---

    class USVLoiterAroundTarget(py_trees.behaviour.Behaviour):
        """USV在目标周围等待"""

        def __init__(self, name: str, agent: Boat):
            super().__init__(name)
            self.agent = agent

        def update(self) -> py_trees.common.Status:
            self.agent.boat_lock_s1()
            return py_trees.common.Status.SUCCESS

    class USVLockAt35km(py_trees.behaviour.Behaviour):
        """USV在35km锁定目标"""

        def __init__(self, name: str, agent: Boat):
            super().__init__(name)
            self.agent = agent

        def update(self) -> py_trees.common.Status:
            self.agent.boat_lock_s2()
            return py_trees.common.Status.SUCCESS


class UAV_BTNodes:
    """白方无人机（UAV）的行为树节点集合"""

    # --- 条件节点 (Conditions) ---

    class IsDocked(py_trees.behaviour.Behaviour):
        """检查UAV是否停靠在USV上"""

        def __init__(self, name: str, agent: Drone):
            super().__init__(name)
            self.agent = agent

        def update(self) -> py_trees.common.Status:
            if self.agent.docking is not None:
                self.feedback_message = f"停靠在 USV {self.agent.docking.id} 上"
                return py_trees.common.Status.SUCCESS
            else:
                self.feedback_message = "正在空中飞行"
                return py_trees.common.Status.FAILURE

    class IfHasTask(py_trees.behaviour.Behaviour):
        """起飞检查UAV是否处于空闲状态"""

        def __init__(self, name: str, agent: Drone):
            super().__init__(name)
            self.agent = agent

        def update(self) -> py_trees.common.Status:
            if self.agent.task_type == TaskType.IDLE and self.agent.rtb_target is self.agent.docking:
                # 检查UAV是否处于空闲状态或停靠目标是否不匹配
                return py_trees.common.Status.RUNNING
            elif self.agent.task_list:
                # 有任务且电量满足基本续航要求
                target_id = self.agent.task_list[0]['task_id']
                target = self.agent.world.faction_maps[self.agent.faction].target_maps[target_id]
                if self.agent.distance_to(target.position) < 0.4 * self.agent.max_speed * self.agent.power:
                    return py_trees.common.Status.SUCCESS
                else:
                    # 否则继续充电
                    return py_trees.common.Status.RUNNING
            elif self.agent.rtb_target is not self.agent.docking or self.agent.rtb_target is None:
                # 停靠母舰不符分配且电量足以飞至新母舰
                if self.agent.rtb_target is None or self.agent.distance_to(
                        self.agent.rtb_target) < 0.8 * self.agent.max_speed * self.agent.power:
                    # 则转场
                    self.feedback_message = "wrong mothership, takeoff"
                    return py_trees.common.Status.SUCCESS
                else:
                    # 否则继续充电
                    self.feedback_message = "wrong mothership but low power, charging..."
                    return py_trees.common.Status.RUNNING
            else:
                return py_trees.common.Status.RUNNING

    class IsFullyCharged(py_trees.behaviour.Behaviour):
        """检查无人机是否电量充足允许执行任务"""

        def __init__(self, name: str, agent: Drone):
            super().__init__(name)
            self.agent = agent

        def update(self) -> py_trees.common.Status:
            if self.agent.state == AgentState.STANDBY:
                self.feedback_message = "无人机电量充足，可以执行任务"
                return py_trees.common.Status.SUCCESS
            else:
                self.feedback_message = "无人机电量不足"
                return py_trees.common.Status.FAILURE

    class IfAttributeUSV(py_trees.behaviour.Behaviour):
        """检查当前UAV是否被分配到了充电USV"""

        def __init__(self, name: str, agent: Drone):
            super().__init__(name)
            self.agent = agent

        def update(self) -> py_trees.common.Status:
            if self.agent.rtb_target is not None:
                self.feedback_message = f"当前UAV被分配到了充电USV"
                return py_trees.common.Status.SUCCESS
            else:
                self.feedback_message = f"当前UAV未被分配到充电USV"
                return py_trees.common.Status.FAILURE

    class IsEnduranceLow(py_trees.behaviour.Behaviour):
        """检查续航是否不足（时间和电量双重检查）"""

        def __init__(self, name: str, agent: Drone):
            super().__init__(name)
            self.agent = agent

        def update(self) -> py_trees.common.Status:
            # 检查电量
            power_ratio = self.agent.power / self.agent.max_power
            if self.agent.power < max(1.25 * self.agent.t_avail, 300):
                self.feedback_message = f"电量低 ({power_ratio:.1%}), 必须返航!"
                self.agent.need_docking = True
                self.agent.task_type = TaskType.RTB
                self.agent.task_list = []  # 取消原有任务
                return py_trees.common.Status.SUCCESS  # 返回SUCCESS以触发返航序列

            self.feedback_message = f"续航充足 ({power_ratio:.1%})"
            return py_trees.common.Status.FAILURE  # 续航充足，返航条件不成立

    class HasAssitLockTask(py_trees.behaviour.Behaviour):
        """检查是否有上层下发的协助锁定任务"""

        def __init__(self, name: str, agent: Drone):
            super().__init__(name)
            self.agent = agent

        def update(self) -> py_trees.common.Status:
            # 假设上层任务存储在 agent.task_list 中，且类型为'move'
            if self.agent.task_list and self.agent.task_type == TaskType.ASSIT_LOCK:
                self.feedback_message = f"接到上层命令: 前往指定坐标"
                return py_trees.common.Status.SUCCESS
            else:
                self.feedback_message = "无上层命令"
                return py_trees.common.Status.FAILURE

    class HasReckonTask(py_trees.behaviour.Behaviour):
        """检查是否有上层下发的移动任务"""

        def __init__(self, name: str, agent: Drone):
            super().__init__(name)
            self.agent = agent

        def update(self) -> py_trees.common.Status:
            if self.agent.task_list and self.agent.task_type == TaskType.RECKON:
                return py_trees.common.Status.SUCCESS
            else:
                return py_trees.common.Status.FAILURE

    class IfIdle(py_trees.behaviour.Behaviour):
        """检查UAV是否处于IDLE状态"""

        def __init__(self, name: str, agent: Drone):
            super().__init__(name)
            self.agent = agent

        def update(self) -> py_trees.common.Status:
            if self.agent.task_type == TaskType.IDLE:
                return py_trees.common.Status.SUCCESS
            else:
                return py_trees.common.Status.FAILURE

    class IfHasRTBTarget(py_trees.behaviour.Behaviour):
        """检查UAV是否被分配到了充电USV"""

        def __init__(self, name: str, agent: Drone):
            super().__init__(name)
            self.agent = agent

        def update(self) -> py_trees.common.Status:
            if self.agent.rtb_target is not None:
                return py_trees.common.Status.SUCCESS
            else:
                return py_trees.common.Status.FAILURE

    # --- 动作节点 (Actions) ---

    class TakeOff(py_trees.behaviour.Behaviour):
        """起飞动作"""

        def __init__(self, name: str, agent: Drone):
            super().__init__(name)
            self.agent = agent

        def update(self) -> py_trees.common.Status:
            if self.agent.docking is not None:
                self.feedback_message = f"从 USV {self.agent.docking.id} 起飞"
                self.agent.take_off()  # 调用已有的take_off方法
                return py_trees.common.Status.SUCCESS
            return py_trees.common.Status.FAILURE  # 如果不在停靠状态，则无法起飞

    class LandOnUSV(py_trees.behaviour.Behaviour):
        """执行降落动作"""

        def __init__(self, name: str, agent: Drone):
            super().__init__(name)
            self.agent = agent

        def update(self) -> py_trees.common.Status:
            target_usv = getattr(self.agent, 'rtb_target', None)
            if target_usv is None:
                # 没有停靠目标直接失败
                self.feedback_message = f"UAV {self.agent.id}无停靠目标，无法降落"
                return py_trees.common.Status.FAILURE

                self.agent.take_off(target_usv.x-self.agent.x, target_usv.y-self.agent.y)

            if target_usv is self.agent.docking:
                self.feedback_message = f"UAV {self.agent.id}成功降落于USV {target_usv.id}"
                self.agent.task_type = TaskType.IDLE
                return py_trees.common.Status.SUCCESS
            else:
                self.agent.take_off(target_usv.x-self.agent.x, target_usv.y-self.agent.y)

            direction = self.agent.angle_to(target_usv)
            distance = self.agent.distance_to(target_usv)

            if distance <= 2000:
                # 满足降落距离，降低速度
                self.feedback_message = f"接近停靠目标，正在减速，准备在 USV {target_usv.id}上降落"
                self.agent.need_docking = True
                self.agent.set_speed(
                    max(self.agent.min_speed, distance * distance / 2000 / 2000 * self.agent.max_speed), direction)
                if target_usv.drone is not None:
                    self.feedback_message = f"USV {target_usv.id}停靠平台被占用，等待泊位空闲"
            else:
                self.agent.set_speed(self.agent.max_speed, direction)
                self.feedback_message = f"距离太远({distance:.0f}m)，正在接近停靠目标"
            return py_trees.common.Status.RUNNING

    class MoveToAssistLock(py_trees.behaviour.Behaviour):
        def __init__(self, name: str, agent: Agent):
            super().__init__(name)
            self.agent = agent

        def update(self) -> py_trees.common.Status:
            if not self.agent.task_list or self.agent.task_type == TaskType.IDLE:
                self.agent.task_type = TaskType.IDLE
                self.feedback_message = "没有任务！"
                return py_trees.common.Status.FAILURE
            target_id = self.agent.task_list[0]['task_id']
            target = None
            for agent in self.agent.world.agents:
                if agent.id == target_id:
                    target = agent
                    break

            if target is None or (target_id in self.agent.world.faction_attacking_cache[self.agent.faction]):
                if self.agent.task_list:
                    self.agent.task_list.pop(0)
                if not self.agent.task_list:
                    self.agent.task_type = TaskType.IDLE
                return py_trees.common.Status.SUCCESS
            else:
                target_map = self.agent.world.faction_maps[self.agent.faction].target_maps[target_id]
                target_pos = target_map.position

                # 计算距离
                pt1 = np.array((self.agent.x, self.agent.y))
                pt2 = np.array(target_pos)
                pt1_2 = pt2 - pt1
                # 大于侦察距离，以最大速度向目标运动
                if np.linalg.norm(pt1_2) >= self.agent.sensor_range:
                    self.agent.set_speed(self.agent.max_speed, math.atan2(pt1_2[1], pt1_2[0]))
                elif np.linalg.norm(pt1_2) > 0:
                    self.agent.set_speed(self.agent.min_speed, math.atan2(pt1_2[1], pt1_2[0]))
                else:
                    pass

                return py_trees.common.Status.RUNNING

    class MoveToUpdateTask(py_trees.behaviour.Behaviour):
        def __init__(self, name: str, agent: Agent):
            super().__init__(name)
            self.agent = agent

        def update(self) -> py_trees.common.Status:
            if not self.agent.task_list or self.agent.task_type == TaskType.IDLE:
                self.agent.task_type = TaskType.IDLE
                self.feedback_message = "没有任务！"
                return py_trees.common.Status.FAILURE
            waypoints = []
            for task in self.agent.task_list[:]:
                target_id = task['task_id']

                target_not_in_world = True
                for agent in self.agent.world.agents:
                    if agent.id == target_id:
                        target_not_in_world = False
                        break

                target_has_been_detected = False
                for agent in self.agent.world.faction_detected[self.agent.faction]:
                    if agent.id == target_id:
                        target_has_been_detected = True
                        break

                if target_not_in_world or target_has_been_detected:
                    self.agent.task_list.pop(0)
                    if not self.agent.task_list:
                        self.agent.task_type = TaskType.IDLE
                        self.agent.world.global_coordinator.last_uav_update_map_allocation_time = -114514
                        self.agent.world.global_coordinator.last_uav_charge_allocation_time = -114514
                        return py_trees.common.Status.SUCCESS
                else:
                    target_map = self.agent.world.faction_maps[self.agent.faction].target_maps[target_id]
                    waypoints.append({'position': target_map.position + [0.0], 'arrival_time': 0})
            self.agent.set_waypoints(waypoints)
            return py_trees.common.Status.RUNNING

    class LoiterHere(py_trees.behaviour.Behaviour):
        def __init__(self, name: str, agent: Agent):
            super().__init__(name)
            self.agent = agent

        def update(self) -> py_trees.common.Status:
            if self.agent.controller.waypoints:
                # 走完尚未完成的路径
                return py_trees.common.Status.RUNNING
            # 无路径点则原地盘旋
            # self.agent.set_heading_to(self.agent)
            return py_trees.common.Status.SUCCESS
